% Copyright 2020 Makani Technologies LLC
%
% Licensed under the Apache License, Version 2.0 (the "License");
% you may not use this file except in compliance with the License.
% You may obtain a copy of the License at
%
%      http://www.apache.org/licenses/LICENSE-2.0
%
% Unless required by applicable law or agreed to in writing, software
% distributed under the License is distributed on an "AS IS" BASIS,
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
% See the License for the specific language governing permissions and
% limitations under the License.

function write_mclaren_matlab_params(mclaren_repo_dirname)
% write_mclaren_matlab_params -- Writes mclaren_matlab_params.py.
%
% See the "McLaren controller" section of go/makani-controls-gs02 for more
% on the McLaren controller.
%
% This function runs the workspace controller to load parameters for the GS02
% McLaren controller into the workspace, then writes them to a Python module
% that returns them in a dictionary, via get_mclaren_matlab_params().

% Load the workspace controller. Note that at time of writing:
%  - setup_path.m requires some user-specific modifications to work properly.
%  - It isn't possible to return to the starting directory at the end of
%    this function, as loadWorkspaceController runs "clear all".
cd(mclaren_repo_dirname);
setup_path;
cd('src/data');
loadWorkspaceController;

output_path = fullfile(getenv('MAKANI_HOME'), ...
  'config/base_station/sim/mclaren_matlab_params.py');
fid = fopen(output_path, 'w');

% These variables will be written to the output file.
varnames = {'aDead_Azi_Transform', ...
  'aDead_Winch_Transform', ...
  'aTargetHT2Reel_Azi_Transform', ...
  'aTargetHT2Reel_Winch_Transform', ...
  'aTargetReel2HT_Azi_Transform', ...
  'aTargetReel2HT_Winch_Transform', ...
  'aTolHT2Reel_Azi_Transform', ...
  'aTolHT2Reel_Winch_Transform', ...
  'aTolReel2HT_Azi_Transform', ...
  'aTolReel2HT_Winch_Transform', ...
  'dnMax_Azi_Transform', ...
  'dnMax_Winch_Reel', ...
  'dnMax_Winch_Transform', ...
  'nNominal_Azi_Transform', ...
  'nNominal_Winch_Transform', ...
  'rDecel_Azi_Transform', ...
  'rDecel_Winch_Transform'};

% Write the header.
fprintf(fid, ...
  ['"""Automatically-retrieved McLaren controller params.', ...
  '\n', ...
  '\nThis file was generated by: write_mclaren_matlab_params.m.', ...
  '\n"""', ...
  'from makani.config import mconfig\n', ...
  '\n', ...
  '\n', ...
  '@mconfig.Config\n', ...
  'def MakeParams():', ...
  '\n  """Returns the Matlab generated McLaren controller params."""
  '\n  return {\'mclaren\': {']);

% Write the output file.
indentation = '      ';
for i = 1:length(varnames)
  varname = varnames{i};
  value = eval(varname);
  if isscalar(value)
    % Print a scalar.
    fprintf(fid, '\n%s''%s'': %#0.3g,', indentation, varname, value);
  else
    % Print an array. "Inf" is made Python-compatible, and any extra
    % whitespace is stripped.
    c = cellstr(num2str(value, '%#0.3g'));
    for j = 1:length(c)
      c{j} = strtrim(c{j});
    end
    str = strjoin(c, ', ');
    str = strrep(str, 'Inf', 'float(''inf'')');
    fprintf(fid, '\n%s''%s'': [%s],', indentation, varname, str);
  end
  fprintf(fid,'}');
end
fprintf(fid, '\n         }\n');

fclose(fid);
end
